import numpy as np
from scipy.io import loadmat
import scipy.io as sio
import os
import math
from scipy.optimize import minimize

# 加载MATLAB数据
def flatten_nested_arrays(data):
    array_m=[]
    for data1 in data:
        processed = []
        for item in data1:
            # 检查是否为NumPy数组且非空
            if isinstance(item, np.ndarray) and item.size > 0:
                # 若数组是二维或更高维，则扁平化
                if item.ndim >= 2:
                    flattened = item.flatten()
                    # print(flattened)
                    processed.append(flattened)
                    # processed.append(flattened.tolist())  # 转换为列表
                else:
                    processed.append(np.array(item.tolist()))
            # 空数组或非数组元素保持原样
            else:
                processed.append(item)
        array_m.append(processed)
    return array_m

# if not os.path.exists('data_OMAS.mat'):
#     print(111111)
def main(T, dt):
    mat_data = loadmat('E:\一年级\科研\论文\mtalab_for_exp_slf_20250622\data_OMAS.mat')
    OMAS_L = mat_data['OMAS_L'][0]
    AGENT = mat_data['AGENT']
    AGENT_A = mat_data['AGENT_A']
    AGENT_X = mat_data['AGENT_X']
    AGENT_V = mat_data['AGENT_V']

    # print(AGENT_X)
    AGENT_X=flatten_nested_arrays(AGENT_X)
    AGENT_V=flatten_nested_arrays(AGENT_V)
    # print(AGENT_V)

    # 算法参数设定
    bar_M = 1  # 动态恶意攻击数量上界
    tau1, tau2 = 0.45, 0.34
    alpha = lambda k: (k + 2)**(-tau1)
    lambda_ = lambda k: 6.9 * (k + 2)**(tau1 - tau2)  # 避免关键字冲突
    umax_1 = 1  # 无人车控制输入限幅
    umax_2 = 1  # 无人机控制输入限幅
    vmax_2 = 0.5
    B = 30  # 二阶系统控制增益

    # 获取时间参数
    total_steps = int(T/dt)-2
    num_agents = len(OMAS_L)

    # 初始化数据结构
    def create_agent_array(shape):
        """创建智能体状态存储数组"""
        return [[[] for _ in range(shape[1])] for _ in range(shape[0])]

    AGENT_U = create_agent_array((num_agents, total_steps+2))
    AGENT_Xhat = create_agent_array((num_agents, total_steps+2))
    AGENT_Xideal = create_agent_array((num_agents, total_steps+2))
    AGENT_Xpredict = create_agent_array((num_agents, total_steps+2))

    # 初始化智能体状态
    for i in range(num_agents):
        agent = OMAS_L[i]
        joining_step = agent['joining_step'][0,0]  # MATLAB索引转Python索引
        
        # 加入时刻的初始化
        AGENT_Xhat[i][joining_step] = AGENT_X[i][joining_step].copy()
        AGENT_Xpredict[i][joining_step] = AGENT_X[i][joining_step].copy()
        
        # 处理不同动力学模型
        dynamics = agent['dynamics'][0,0]
        
        if dynamics == 1:  # 一阶系统
            # 控制输入初始化
            AGENT_U[i][joining_step] = np.array([0.0, 0.0])
            
            # 下一时刻状态初始化
            next_step = joining_step + 1
            if next_step < total_steps:
                AGENT_Xhat[i][next_step] = AGENT_X[i][joining_step].copy()
                AGENT_Xpredict[i][next_step] = AGENT_X[i][joining_step].copy()
        
        elif dynamics == 2:  # 二阶系统
            next_step = joining_step + 1
            if next_step < total_steps:
                # 计算初始速度状态
                velocity = AGENT_V[i][joining_step] if joining_step < len(AGENT_V[i]) else np.zeros(2)
                print(velocity)
                # 计算下一时刻状态
                next_state = AGENT_X[i][joining_step] + dt * velocity
                
                AGENT_Xhat[i][next_step] = next_state.copy()
                AGENT_Xpredict[i][next_step] = next_state.copy()

    # 验证初始化结果
    print(f"初始化完成，共{num_agents}个智能体")
    print(f"时间总步数: {total_steps} (T={T}, dt={dt})")
    print("示例智能体初始状态:")
    print(f"AGENT_Xhat[0] = {AGENT_Xhat[1][1]}")
    print(f"AGENT_Xpredict[0] = {AGENT_Xpredict[1][1]}")
    print(f"AGENT_U[0][0] = {AGENT_U[2][0]}")

    for k in range(total_steps):
        # 当前时刻数据
        current_step = k
        AGENT_tmp = np.where(AGENT[:, current_step] >= 1)[0].tolist()
        AGENT_N_tmp = np.where(AGENT[:, current_step] == 1)[0].tolist()
        AGENT_M_tmp = np.where(AGENT[:, current_step] == 2)[0].tolist()
        
        # print(AGENT_tmp,AGENT_N_tmp,AGENT_M_tmp)
        AGENT_h_tmp = {i: None for i in range(len(OMAS_L))}
        for i in AGENT_tmp:
            idx = AGENT_tmp.index(i)
            AGENT_h_tmp[i] = function_hL(idx, len(AGENT_tmp), k, dt)
        
        alpha_tmp = alpha(k)
        lambda_tmp = lambda_(k)
        
        # 下一时刻数据
        if k != total_steps:
            next_step = k + 1
            AGENT_next_tmp = np.where(AGENT[:, next_step] >= 1)[0].tolist()
            AGENT_N_next_tmp = np.where(AGENT[:, next_step] == 1)[0].tolist()
            AGENT_M_next_tmp = np.where(AGENT[:, next_step] == 2)[0].tolist()
        else:
            AGENT_next_tmp = AGENT_tmp.copy()
            AGENT_N_next_tmp = AGENT_N_tmp.copy()
            AGENT_M_next_tmp = AGENT_M_tmp.copy()
        
        AGENT_h_next_tmp = {i: None for i in range(len(OMAS_L))}
        for i in range(len(OMAS_L)):
            if i in AGENT_next_tmp:
                idx = AGENT_next_tmp.index(i)
                AGENT_h_next_tmp[i] = function_hL(idx, len(AGENT_next_tmp), k+1, dt)
            elif i in list(set(AGENT_tmp) - set(AGENT_next_tmp)):
                AGENT_h_next_tmp[i] = AGENT_h_tmp[i]
        
        alpha_next_tmp=alpha(k+1)
        lambda_next_tmp=lambda_(k+1)

        # 时刻k+2的相关数据
        if k != total_steps and k != total_steps-1:
            next_step = k + 2
            AGENT_next_tmp2 = np.where(AGENT[:, next_step] >= 1)[0].tolist()
        else:
            AGENT_next_tmp2 = AGENT_next_tmp

        AGENT_h_next_tmp2 = {i: None for i in range(AGENT.shape[0])} 
        # 各智能体时刻k+1的编队偏移【N*1元组，各元组为2*1向量（第i个元组代表智能体i在时刻k+1的编队偏移）】（注：非时刻k或非时刻k+2参与的智能体对应的元组为空集）
        for i in range(len(OMAS_L)):
            if i in AGENT_next_tmp2:
                idx = AGENT_next_tmp2.index(i)
                AGENT_h_next_tmp2[i]=function_hL(idx,len(AGENT_next_tmp2),k+2,dt)
            elif i in list(set(AGENT_next_tmp) - set(AGENT_next_tmp2)):
                AGENT_h_next_tmp2[i]=AGENT_h_next_tmp[i]
            

        # 处理不同阶数的智能体
        # 1阶正常智能体
        for i in AGENT_N_next_tmp:
            agent = OMAS_L[i]
            if agent['dynamics'][0,0] == 1:
                # 状态更新和控制逻辑
                if AGENT_Xhat[i][k] is None:
                    AGENT_Xhat[i][k] = AGENT_X[i][k]
                    AGENT_Xhat[i][k+1] = AGENT_X[i][k]
                
                if k != total_steps:
                    # print(AGENT_A,k)
                    neighbors = np.where(AGENT_A[k+1][0][i] == 1)[0].tolist()
                else:
                    neighbors = np.where(AGENT_A[k][0][i] == 1)[0].tolist()
                
                # 邻居交互处理
                # AGENT_X_tmp = create_agent_array((num_agents, 1))
                AGENT_X_tmp = {}
                invalid_neighbors = []
                for j in neighbors:
                    if j in AGENT_N_next_tmp:
                        AGENT_X_tmp[j] = AGENT_Xpredict[j][k+1]
                        # print(AGENT_X_tmp,"1111")
                    else:
                        AGENT_X_tmp[j] = function_attack(AGENT_Xpredict[i][k+1], 
                                                        AGENT_Xpredict[i][k-65+1], 
                                                        AGENT_Xpredict[j][k+1], i, k+1, dt)
                    if len(AGENT_X_tmp[j])==0:
                        invalid_neighbors.append(j)
                
                valid_neighbors = [j for j in neighbors if j not in invalid_neighbors]
                
                # print(AGENT_X_tmp,neighbors,AGENT_N_next_tmp,"1111")
                # 状态更新
                # print(neighbors,AGENT_X_tmp,AGENT_h_next_tmp)
                AGENT_Xhat[i][k+2] = function_update(
                    bar_M - len(invalid_neighbors),
                    AGENT_Xpredict[i][k+1],
                    AGENT_X_tmp,
                    valid_neighbors,
                    AGENT_h_next_tmp,
                    alpha_tmp,
                    lambda_tmp,
                    i,
                    k+1,
                    AGENT_h_next_tmp2.get(i),dt
                )
                # print(AGENT_Xpredict[i][k+1],AGENT_Xhat[i][k+2])
                # 控制输入计算 目前已完成到这里
                AGENT_U[i][k+1] = function_control_1(
                    AGENT_Xpredict[i][k+1],
                    AGENT_Xhat[i][k+2],
                    umax_1,dt
                )
                # print(AGENT_U[i][k+1])
                # 状态预测 k还没改
                AGENT_Xpredict[i][k+2] = AGENT_Xpredict[i][k+1] + dt * AGENT_U[i][k+1]
                # print

                if i in AGENT_tmp:
                    AGENT_X[i][k+1], AGENT_V[i][k+1] = function_trans_1(
                        AGENT_X[i][k],
                        AGENT_U[i][k],
                        dt
                    )
                if np.linalg.norm(AGENT_X[i][k+1],ord=2) > 100000:
                    print(111111111111111111)

        # 恶意一阶智能体处理
        for i in AGENT_M_next_tmp:
            agent = OMAS_L[i]
            if agent['dynamics'][0,0] == 1:
                # 初始化理想状态
                if len(AGENT_Xideal[i][k])==0:
                    AGENT_Xideal[i][k] = AGENT_X[i][k].copy()
                    AGENT_Xideal[i][k+1] = AGENT_X[i][k].copy()
                
                # 获取邻居信息
                if k != total_steps:
                    neighbors = np.where(AGENT_A[k+1][0][i] == 1)[0].tolist()
                else:
                    neighbors = np.where(AGENT_A[k][0][i] == 1)[0].tolist()
                
                # 排除自身后的邻居集合
                filtered_neighbors = [j for j in neighbors if j != i]
                
                # 收集邻居信息
                AGENT_X_tmp = {}
                invalid_neighbors = []
                for j in filtered_neighbors:
                    if j in AGENT_N_next_tmp:
                        AGENT_X_tmp[j] = AGENT_Xpredict[j][k+1]
                    else:
                        AGENT_X_tmp[j] = function_attack(
                            AGENT_Xpredict[i][k+1],
                            AGENT_Xpredict[i][k-65+1],  # 修正索引偏移: k-65+2 = k-63
                            AGENT_Xpredict[j][k+1],
                            i, k+1, dt
                        )
                    if len(AGENT_X_tmp[j])==0:
                        invalid_neighbors.append(j)
                
                # 更新有效邻居列表
                valid_neighbors = [j for j in filtered_neighbors if j not in invalid_neighbors]
                
                # print(AGENT_X_tmp,"2222")
                # 状态更新（恶意版本）
                xhat_ideal = function_update_malicious(
                    bar_M - len(invalid_neighbors),
                    AGENT_Xideal[i][k+1],
                    AGENT_X_tmp,
                    valid_neighbors,
                    AGENT_h_next_tmp,
                    i,
                    AGENT_h_next_tmp2.get(i)
                )
                
                # 理想控制量计算
                u_tmp_ideal = function_control_1(
                    AGENT_Xideal[i][k+1],
                    xhat_ideal,
                    umax_1,dt
                )
                
                # 更新理想状态
                AGENT_Xideal[i][k+2] = AGENT_Xideal[i][k+1] + dt * u_tmp_ideal
                
                # 控制增益计算
                if k * dt > 10:
                    u_gain = (1 + 0.05 * np.sin((k+1)*dt)) * (1 - 0.1 * np.exp(-(k+1)*dt))
                else:
                    u_gain = 1.0
                
                # 实际控制量应用
                AGENT_U[i][k+1] = u_gain * u_tmp_ideal
                
                # 状态预测
                AGENT_Xpredict[i][k+2] = AGENT_Xpredict[i][k+1] + dt * AGENT_U[i][k+1]
                
                # 实际状态转移
                AGENT_X[i][k+1], AGENT_V[i][k+1] = function_trans_1(
                    AGENT_X[i][k],
                    AGENT_U[i][k],
                    dt
                )

                if np.linalg.norm(AGENT_X[i][k+1],ord=2) > 100000:
                    print(111111111111111111)

        # 2阶正常智能体
        for i in AGENT_N_next_tmp:
            agent = OMAS_L[i]
            if agent['dynamics'][0,0] == 2:
                
                if k != total_steps:
                    neighbors = np.where(AGENT_A[k+1][0][i] == 1)[0].tolist()
                else:
                    neighbors = np.where(AGENT_A[k][0][i] == 1)[0].tolist()
                
                # 邻居交互处理
                AGENT_X_tmp = {}
                invalid_neighbors = []
                for j in neighbors:
                    if j in AGENT_N_next_tmp:
                        AGENT_X_tmp[j] = AGENT_Xpredict[j][k+1]
                        # print(AGENT_X_tmp,"2222")
                    else:
                        AGENT_X_tmp[j] = function_attack(AGENT_Xpredict[i][k+1], 
                                                        AGENT_Xpredict[i][k-65+1], 
                                                        AGENT_Xpredict[j][k+1], i, k+1, dt)
                    if len(AGENT_X_tmp[j])==0:
                        invalid_neighbors.append(j)
                
                valid_neighbors = [j for j in neighbors if j not in invalid_neighbors]
                # print(AGENT_X_tmp,"3333")
                # 状态更新
                AGENT_Xhat[i][k+2] = function_update(
                    bar_M - len(invalid_neighbors),
                    AGENT_Xpredict[i][k+1],
                    AGENT_X_tmp,
                    valid_neighbors,
                    AGENT_h_next_tmp,
                    alpha_tmp,
                    lambda_tmp,
                    i,
                    k+1,
                    AGENT_h_next_tmp2.get(i),dt
                )
                # print(AGENT_Xhat[i][k+2])
                # 控制输入计算
                AGENT_U[i][k] = function_control_2(
                    AGENT_X[i][k],
                    AGENT_V[i][k],
                    AGENT_Xpredict[i][k+1],
                    AGENT_Xhat[i][k+2],
                    umax_2,
                    vmax_2,B,dt
                )

                # print(AGENT_U[i][k])

                # 时刻k+2的预计算状态
                v_tmp=AGENT_V[i][k]+B*AGENT_U[i][k]*dt
                # 状态预测
                AGENT_Xpredict[i][k+2] = AGENT_Xpredict[i][k+1] + dt * v_tmp
                
                if (i in AGENT_N_tmp) and (i in AGENT_N_next_tmp) :
                    AGENT_X[i][k+1], AGENT_V[i][k+1] = function_trans_2(
                        AGENT_X[i][k],
                        AGENT_V[i][k],
                        B*AGENT_U[i][k],
                        dt
                    )
                
                if np.linalg.norm(AGENT_X[i][k+1],ord=2) > 100000:
                    print(111111111111111111)

    print(f"运算完成")
    # 将变量打包成字典（键为字符串形式的变量名，值为对应的数据）
    data_to_save = {
        'T': T,
        'dt': dt,
        'AGENT_X': AGENT_X,
        'AGENT_V': AGENT_V,
        'AGENT_Xhat': AGENT_Xhat,
        'AGENT_Xideal': AGENT_Xideal
    }

    sio.savemat('data_result_py.mat', data_to_save)

# def function_fL(i, x, k, T, dt):
#     """
#     本地目标函数f实现
    
#     参数:
#     i : int - 智能体索引号 (1-based MATLAB索引保持原样)
#     x : numpy.ndarray - 智能体状态 (2,)
#     k : int - 当前时刻 (时间步索引)
    
#     返回:
#     float - 目标函数值
#     """
    
#     # 计算基础参考点
#     xc_L = np.array([
#         3 + 3 * np.sin(-(np.pi/2) * (4/T) * k * dt),
#         2 + 2 * np.sin(-np.pi * (4/T) * k * dt)
#     ])
    
#     # 添加个体偏移量
#     offset = np.array([
#         (-0.015 + 0.03 * (i+1)/7) * np.sin(np.pi * k * dt / 40),
#         (-0.02 + 0.04 * (i+1)/7) * np.sin(np.pi * k * dt / 20)
#     ])
    
#     xc_L += offset
    
#     # 计算目标函数值
#     return 0.5 * np.sum((x - xc_L)**2)

def function_gL(i, x, k):
    """本地目标函数g实现"""
    g=0.05*np.linalg.norm(x,ord=2)
    return g

def function_hL(i, num_AGENT, k, dt):
    """
    编队偏移向量计算函数
    
    参数:
    i : int - 智能体索引
    num_AGENT : int - 当前参与的智能体总数
    k : int - 当前时间步索引
    dt : float - 时间间隔
    
    返回:
    numpy.ndarray - 二维偏移向量 [2,]
    """
    angle = 2 * np.pi * i / num_AGENT + np.pi * k * dt / 80
    h_L = 0.7 * np.array([np.cos(angle), np.sin(angle)])
    return h_L

# def function_Pglobal(x):
#     """全局目标函数实现"""
#     return 0.0

# def function_Pglobal(x):
#     """
#     全局目标函数实现
    
#     参数:
#     x : numpy.ndarray - 优化变量向量
    
#     返回:
#     float - 目标函数值
#     """
#     global f_L, g_L, AGENT_N_tmp, k_tmp
    
#     P = 0.0
#     c = 0
    
#     for i in AGENT_N_tmp:
#         # 提取当前智能体的状态变量
#         x_i = x[c:c+2]
        
#         # 计算f和g的函数值
#         f_val = f_L[i](x_i, k_tmp)
#         g_val = g_L[i](x_i, k_tmp)

#         # print(f_val,g_val,"!!!!!!!")
        
#         # 累加到目标函数
#         P += f_val + g_val
        
#         # 更新变量索引
#         c += 2
    
#     return P

def function_attack(xi,xi_past,xj,i,k,dt):
    if k*dt<=9:
        # print(xj,xi)
        # print(1111)
        x=xj+math.sin((i+1)*k*dt)*np.max(np.vstack((xj,xi)),axis=0) # FDI（错误数据注入）攻击
    elif k*dt>9 and k*dt<=25:
        x=[] # DOS（拒绝服务）攻击
    elif k*dt>25 and k*dt<=26:
        x=xj+6.8*math.cos(k*dt)*np.ones((1,2))[0] # FDI（错误数据注入）攻击
        # print(6.8*math.cos(k*dt)*np.ones((1,2))[0])
        # print(3333)
    else:
        # print(4444)
        x=xi_past # 重放攻击
    # print(x)
    return x

def function_update(bar_M, x_tmp, AGENT_X_tmp, Neighbor_tmp, AGENT_h_tmp, alpha_tmp, lambda_tmp, i, k, h_next_tmp, dt):
    """
    估计值更新函数
    
    参数:
    bar_M -- 弹性邻居数量
    x_tmp -- 当前状态估计
    AGENT_X_tmp -- 邻居状态信息字典
    Neighbor_tmp -- 有效邻居索引列表
    AGENT_h_tmp -- 编队偏移字典
    alpha_tmp -- 步长参数
    lambda_tmp -- 正则化参数
    i -- 当前智能体索引
    k -- 当前时间步
    h_next_tmp -- 下一时刻编队偏移
    
    返回:
    xhat -- 更新后的状态估计
    """
    # global z_tmp, a_ttmp
    a_ttmp = alpha_tmp
    h_tmp = AGENT_h_tmp.get(i, np.zeros((2,1)))
    X = np.array([[6], [4]])

    # print(AGENT_X_tmp.get(i, np.zeros((2,1))),h_tmp,i)
    # 1. 邻居的弹性加权信息
    eij = []
    for j in Neighbor_tmp:
        xi = AGENT_X_tmp.get(i, np.zeros((2,1))) - h_tmp
        xj = AGENT_X_tmp.get(j, np.zeros((2,1))) - AGENT_h_tmp.get(j, np.zeros((2,1)))
        # print(AGENT_X_tmp.get(j, np.zeros((2,1))),AGENT_h_tmp.get(j, np.zeros((2,1))))
        # print(xi, xj)
        eij.append((xi - xj))
    
    distances = [np.linalg.norm(e) for e in eij]
    sorted_indices = np.argsort(distances)
    c_tmp = max(1, len(Neighbor_tmp) - bar_M)
    e_bar = np.array(np.sum([eij[idx] for idx in sorted_indices[:c_tmp]], axis=0) / c_tmp)

    # 2. 辅助向量更新
    df = function_dfL(i, AGENT_X_tmp.get(i, np.zeros((2,1))), k ,dt)
    z_tmp = x_tmp - h_tmp +- alpha_tmp * (df + lambda_tmp * e_bar)

    # 3. 近端算子优化
    a = 0.05
    b = 1 / (2 * a_ttmp)
    bounds = [(0, 6), (0, 4)]  # 状态约束

    # 定义优化目标函数
    def objective(r):
        return a * np.linalg.norm(r) + b * np.sum((r - z_tmp)**2)

    # 使用SLSQP算法进行优化
    initial_guess = np.array([1, 1])
    result = minimize(objective, initial_guess, method='SLSQP', bounds=bounds)
    ropt_tmp = result.x.reshape(-1,1)
    ropt_tmp = ropt_tmp.flatten()
    # print(ropt_tmp+ h_next_tmp)
    return ropt_tmp + h_next_tmp

def function_dfL(i, x, k, dt):
    """
    计算本地目标函数的次梯度
    
    参数:
    i -- 智能体索引号 (int)
    x -- 当前状态 (numpy.ndarray, shape=(2,1))
    k -- 当前时间步 (int)
    
    返回:
    df -- 次梯度 (numpy.ndarray, shape=(2,1))
    """
    T = 40
    # 计算基础参考位置
    xc_L = np.array([
        3 + 3 * np.sin(-(np.pi/2) * (4/T) * k * dt),
        2 + 2 * np.sin(-np.pi * (4/T) * k * dt)
    ])
    # 添加个体偏移量
    offset = np.array([
        (-0.015 + 0.03 * (i+1)/7) * np.sin(np.pi * k * dt / 40),
        (-0.02 + 0.04 * (i+1)/7) * np.sin(np.pi * k * dt / 20)
    ])
    xc_L += offset
    # 计算次梯度
    df = x - xc_L
    
    return df

def function_update_malicious(bar_M, x_tmp, AGENT_X_tmp, Neighbor_ttmp, AGENT_h_tmp, i, h_next_tmp):
    """
    恶意智能体的估计值更新函数
    
    参数:
    bar_M -- 弹性邻居数量
    x_tmp -- 当前状态估计 (numpy.ndarray, shape=(2,1))
    AGENT_X_tmp -- 邻居状态字典 {邻居索引: 状态值}
    Neighbor_ttmp -- 有效邻居索引列表
    AGENT_h_tmp -- 编队偏移字典 {智能体索引: 偏移值}
    i -- 当前智能体索引
    h_next_tmp -- 下一时刻编队偏移 (numpy.ndarray, shape=(2,1))
    
    返回:
    xhat -- 更新后的状态估计 (numpy.ndarray, shape=(2,1))
    """
    # 获取当前编队偏移
    h_tmp = AGENT_h_tmp.get(i, np.zeros((2,1)))
    # print(x_tmp ,h_tmp)
    
    # 1. 计算邻居的弹性加权信息
    eij = []
    for j in Neighbor_ttmp:
        xi = x_tmp - h_tmp
        xj = AGENT_X_tmp.get(j, np.zeros((2,1))) - AGENT_h_tmp.get(j, np.zeros((2,1)))
        # print(AGENT_X_tmp.get(j, np.zeros((2,1))),AGENT_h_tmp.get(j, np.zeros((2,1))))
        # print(xi, xj)
        eij.append((xi - xj))
    
    # 处理空邻居情况
    distances = [np.linalg.norm(e) for e in eij]
    sorted_indices = np.argsort(distances)
    # e_bar = np.array(np.sum([eij[idx] for idx in sorted_indices[:c_tmp]], axis=0) / c_tmp)

    # 计算有效邻居数量
    c_tmp = max(0, len(Neighbor_ttmp) - (bar_M - 1))
    
    if c_tmp > 0:
        # 取前c_tmp个最近邻居
        # print(eij)
        e_bar = np.array(np.sum([eij[idx] for idx in sorted_indices[:c_tmp]], axis=0) / c_tmp)
    else:
        e_bar = np.zeros((2,1))
    
    # 2. 更新估计值
    xhat = (x_tmp - h_tmp) + h_next_tmp - e_bar
    
    return xhat

def function_control_1(x_tmp,xhat_tmp,umax,dt):
    # FUNCTION_CONTROL_1 一阶智能体控制器
    u_tmp=(1/dt)*(xhat_tmp-x_tmp)
    # print(u_tmp)
    for un in range(len(u_tmp)):
        if u_tmp[un]>umax:
            u_tmp[un]=umax
        elif u_tmp[un]<-umax:
            u_tmp[un]=-umax

    return u_tmp

def function_control_2(x_tmp,v_tmp,x_tmp2,xhat2,umax,vmax,B,dt):

    # 控制协议
    u_tmp=(1/(B*dt**2))*(xhat2-2*x_tmp2+x_tmp)
    # 控制协议限幅
    v_tmp2=v_tmp+B*u_tmp*dt
    for vn in range(len(v_tmp2)):
        if v_tmp2[vn]>vmax:
            u_tmp[vn]=(vmax-v_tmp[vn])/(B*dt)
        elif v_tmp2[vn]<-vmax:
            u_tmp[vn]=(-vmax-v_tmp[vn])/(B*dt)

    for un in range(len(u_tmp)):
        # print(un)
        if u_tmp[un]>umax:
            u_tmp[un]=umax
        elif u_tmp[un]<-umax:
            u_tmp[un]=-umax

    return u_tmp

def function_trans_1(x,u,dt):
    x1=x+u*dt
    v1=(x1-x)/dt
    return x1,v1

def function_trans_2(x,v,u,dt):
    x1=x+v*dt
    v1=v+u*dt
    return x1,v1

if __name__ == "__main__":
    T = 40
    dt = 0.01
    main(T, dt)